/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Behaviors.Schemas.Perception;

import Behaviors.Schemas.*;
import Behaviors.Schemas.Perception.*;
import Util.*;
import Robot.*;
import java.util.Vector;
import java.awt.Color;
/**
 *
 * @author alejo
 */
public class PSMapObstacles extends PSDetectObstacles{

    Schema detect;
    Position2d position;
    Vector obstacles;
    //Rango en el cual se considera que los obstaculos todavia existen.
    float MEMORY_RANGE = 2.0f;
    float OBST_SIZE = 0.05f;

    VectorFrame frame;
    boolean showFrame;


    public PSMapObstacles(String name,Schema detect, Position2d position, boolean showFrame)
    {
        super(name);
        this.detect = detect;
        this.position = position;
        this.showFrame = showFrame;
        obstacles = new Vector();
        if(showFrame)
            configFrame();
        
    }

    public PSMapObstacles(String name,Schema detect, Position2d position, float memRange, float obstSize, boolean showFrame)
    {
        super(name);
        this.detect = detect;
        this.position = position;
        this.showFrame = showFrame;
        obstacles = new Vector();

        if(memRange != -1)
            this.MEMORY_RANGE = memRange;
        if(obstSize != -1)
            this.OBST_SIZE = obstSize;
        if(showFrame)
            configFrame();

    }

    public void configFrame()
    {
        frame = new VectorFrame("Obstaculos",VectorFrame.POLAR_COORD,10,18);

        //draw circle that represents the robot
        frame.addStaticElement("robot", frame.CIRCLE_ELEMENT, Robot.radius, 0.0f, 0.0f, Color.RED);
        //draw circle that represents memory limit
        frame.addStaticElement("memoryLimit",frame.CIRCLE_ELEMENT, MEMORY_RANGE, 0, 0,Color.ORANGE);
        //dont draw table
        frame.drawTable(false);
        frame.setVisible(true);
    }


    public float getMaxRange()
    {
        return MEMORY_RANGE;
    }

    public void setMaxRange(float range)
    {
        this.MEMORY_RANGE = range;
    }


    public float getObstSize()
    {
        return OBST_SIZE;
    }

    public void setObstSize(float obstsize)
    {
        this.OBST_SIZE = obstsize;
    }

    public Vector2d[] Value()
    {
        Vector2d[] newObst = ((PSDetectObstacles)detect).Value();


        Vector2d robotPos = position.getPositionVector();
        int length = newObst.length;


        
        //first assign directly all obstacles detected by sensors to the array
        for(int i=0;i<length;i++)
        {
            
            if(newObst[i].getMag()>0)
            {
                //review each obstacles in the array in order to avoid redundant obstacles
                if(obstacles.size()!=0)
                {
                     boolean isNew = true;
                    for(int j=0;j<obstacles.size();j++)
                    {
                        
                        //get obstacles that are already in the array
                        Vector2d tempObst = new Vector2d((Vector2d)obstacles.get(j));
                        //calculate difference between the new obstacle
                        Vector2d diffObst = newObst[i].sub(tempObst);
                        
                        if(Math.abs(diffObst.getMag())<this.OBST_SIZE)
                        {
                            isNew = false;
                            break;

                        }
                            

                    }
                    if(isNew)
                    {
                        obstacles.add(new Vector2d(newObst[i]));
                    }
                }
                //First obstacle added to the array
                else
                    obstacles.add(new Vector2d(newObst[i]));
                
            }
        }

        //now review all obstacles, to see which ones are in the area of influence.
        int numObst = obstacles.size();
        for(int i=0;i<numObst;i++)
        {
            Vector2d tempObst = (Vector2d)obstacles.get(i);
            Vector2d distToObst = tempObst.sub(robotPos);
            if(Math.abs(distToObst.getMag())>MEMORY_RANGE)
            {
                obstacles.remove(i);
                numObst--;
                i--;
            }

            if(obstacles.size()<1)
                break;

        }

        Vector2d[] result = (Vector2d[])obstacles.toArray(new Vector2d[obstacles.size()]);

        if(showFrame)
        {
            Vector2d[] graphVects = new Vector2d[result.length];

                frame.resetDynElements();

            for(int i=0;i<result.length;i++)
            {
                graphVects[i] = result[i].sub(robotPos);
                frame.addDynElement("", frame.CIRCLE_ELEMENT, OBST_SIZE, graphVects[i].getX(), graphVects[i].getY(), Color.RED);
            }
        }

        //Calculate orientation of robot
        Pose2d pos = position.read(Position2d.READ_MODE_NO_WAIT);

        //Direction vector of the robot
        if(showFrame)
        {
            frame.addDynElement("",frame.VECTOR_ELEMENT,0, (float)Math.cos(pos.getPa()), (float)Math.sin(pos.getPa()),Color.BLUE);
            frame.repaint();
        }

        if(result == null)
            return new Vector2d[0];
        else
            return result;
        




    }

}
